# -*- code:utf-8 -*-

# read gps data from a serial
import numpy as np
import serial
import time
import re

DEBUG = True
def gpsread(port,datamaxlength=1000):
    if port is None:
        return None
    # read gps data from serial port
    if(datamaxlength==0):
            return None
    try:
        dev = serial.Serial(port,115200,timeout=10)
    except:
        print 'Some errors happens, check your permission for and device avaliability'
    # databuffer = ''
    gpsdatas = np.zeros((datamaxlength,3))
    dataIndex= 0
    while True:
        time.sleep(0.01)
        data  = dev.readline()
        a=re.search(r'(?:\$GPYBM,SN\d+,[\d\.]+,)([-\+\.\d]+,[-\+\.\d]+,[-\+\.\d]+)',data)
        if a is not None:
            if DEBUG:
                print 'data find:%d'%dataIndex
            lladata = a.group(1).split(',')
            digitdata = np.zeros((3,),dtype=float)
            welldone=True
            for i in range(0,3):
                try:
                    digitdata[i]=float(lladata[i])
                except:
                    welldone=False
                    break
            if welldone :
                gpsdatas[dataIndex,:] = digitdata
                dataIndex=dataIndex+1
                if(dataIndex==datamaxlength):
                    if DEBUG:
                        print 'break:gpsread'
                    break
    return gpsdatas
def uwbread(port,datamaxlength=1000,anchorNum=3):
    if port is None:
        return None
    # read gps data from serial port
    if(datamaxlength==0):
            return None
    try:
        dev = serial.Serial(port,115200,timeout=10)
    except:
        print 'Some errors happens, check your permission for and device avaliability'
    # databuffer = ''
    uwbdatas = np.zeros((datamaxlength,anchorNum))
    dataIndex= np.zeros((anchorNum,))
    
    while True:
        time.sleep(0.005)
        data  = dev.readline()
        a=re.search(r'distance (\d+):\s*(\d+)mm',data)
        if a is not None:
            if DEBUG:
                print 'data find:',dataIndex
            anchorIndex=int(a.group(1))-1
            if(anchorIndex+1>anchorNum):
                raise Exception('unexpected anchor')
            anchorDataIndex = int(dataIndex[anchorIndex])
            uwbdatas[anchorDataIndex,anchorIndex] = int(a.group(2))
            dataIndex[anchorIndex] = dataIndex[anchorIndex] +1
            if(dataIndex[anchorIndex]==datamaxlength):
                break
    return uwbdatas

    
if __name__ == "__main__":
    print "gps read example"
    import threading,Queue
    import scipy.io
    import winsound

    satrecordFileprefix='exp1_gpsbase_'
    timenow = time.time()
    gpsbaseCOM = 'com8'
    # gpsagentCOM = 'com4'
    gpsagentCOM='com11'
    # uwbCOM     = 'com6'
    uwbCOM='com3'
    deviceCOM=[gpsbaseCOM,gpsagentCOM,uwbCOM]
    dataname =['gpsbase','gpsagent','uwb']
    # deviceCOM=[gpsbaseCOM]
    # uwbread(uwbCOM)

    def qeuenWarper1(q,devreadfun,port,name,lengthmax=1000):
        result= devreadfun(port,lengthmax)
        q.put({'name':name,'result':result})
    funcs =[gpsread,gpsread,uwbread]
    # funcs =[gpsread]
    q = Queue.Queue()
    lengthmax=[300,300,1000]
    for ind,func in enumerate(funcs):
        t= threading.Thread(target=qeuenWarper1,args=(q,func,deviceCOM[ind],dataname[ind],lengthmax[ind]))
        # t.daemon = True
        t.start()
        # t.join()
    while threading.activeCount()!=1:
        print 'threads:',threading.activeCount()
        time.sleep(0.5)
    s=list()
    s.append( q.get())
    s.append( q.get())
    s.append( q.get())

    for dataset in s:
        if dataset['name'] == 'gpsbase':
            gpsbasedata= dataset['result']
            if gpsbasedata is None:
                gpsbasedata=np.array([])
        if dataset['name'] == 'gpsagent':
            gpsagentdata= dataset['result']
            if gpsagentdata is None:
                gpsagentdata=np.array([])
        if dataset['name'] == 'uwb':
            uwbdata= dataset['result']
            if uwbdata is None:
                uwbdata=np.array([])
    scipy.io.savemat(satrecordFileprefix+str(int(timenow))+'.mat', {'gpsbase':gpsbasedata, 'gpsagent':gpsagentdata,'uwb':uwbdata})
    # for i in range(1,100):
    #     winsound.MessageBeep(winsound.MB_ICONHAND)
    #     time.sleep(2)
    # winsound.PlaySound('1.wav',winsound.SND_FILENAME)
    # final_gpsbase_Result =gpsbaseResult.get()
    # final_gpsagent_Result =gpsagentResult.get()
    # final_uwb_Result =uwbResult.get()
    # scipy.io.savemat(satrecordFileprefix+str(int(time.time()))+'.mat', {'gpsbaseResult':final_gpsbase_Result, 'gpsagentResult':final_gpsagent_Result,'uwbResult':final_uwb_Result})